import cv
import numpy as np
import cPickle
import math

gestures = ['fingers_spread','fist','fingers_closed','thumb_pinky', \
        'peace', 'point1finger','point1fingerthumb', \
        'point2fingers','point2fingersthumb']
act = [0,1,1]
k = 3

def getFeatures(frame, type):
    # General
    af = np.sum(frame)
    xs = np.size(frame,1)
    ys = np.size(frame,0)
    xi = np.repeat([np.arange(xs)],ys,axis=0)
    yi = np.repeat([np.arange(ys)],xs,axis=0).transpose()
    xc = np.sum(xi*frame)/af
    yc = np.sum(yi*frame)/af

    #Feature Vec 1
    at = xs*ys
    framet = frame.transpose()
    xv =xs/2
    yv =ys/2
    xr = (np.sum(framet[:xv])*-1)+np.sum(framet[xv:])
    yr = (np.sum(frame[:yv])*-1)+np.sum(frame[xv:])

    feat1 = (float(af)/at, float(xs)/ys, float(xr)/(at/2), float(yr)/(at/2))

    #Feature Vec 2
    feat2 =cv.GetHuMoments(cv.Moments(frame,binary=1))

    #Feature Vec 3
    bins = 20
    st = float(1)/bins
    lb = np.arange(bins)*st
    ub = (np.arange(bins)+1)*st
    feat3 = np.zeros(bins)

    frame1 = frame * 255
    frame2 = np.zeros(frame.shape,dtype='uint8')
    cv.Canny(frame1,frame2,255,255)
    (ye,xe) = np.nonzero(frame2)

    d = ((xe-xc)**2 + (ye-yc)**2)**0.5
    dn = np.divide(d,math.sqrt(xs**2+ys**2))

    for i in range(bins):
        feat3[i] = np.sum((dn>=lb[i])==(dn<ub[i]))

    feat3 = tuple(feat3/np.sum(feat3))
    
    return (feat1,feat2,feat3,type)

def displayFrame(type,fn):
    file = open('/scratch/kinect_data/'+type+'.depth','r')
    depthFrames = cPickle.load(file)
    file.close()
    frame = depthFrames[fn]
    frame = frame.astype('uint8')
    frame = frame * 255
    
    image = cv.CreateImageHeader((frame.shape[1], frame.shape[0]), cv.IPL_DEPTH_8U,1)
    cv.SetData(image, frame.tostring(), frame.dtype.itemsize * frame.shape[1])
    cv.ShowImage(type, image)
    cv.WaitKey(40000)

def dist(t1,t2,a):
    d = 1
    for i in range(len(a)):
        if a[i]:
            p = np.array(t1[i])
            q = np.array(t2[i])
            d *= math.sqrt(np.sum((p-q)**2))
    return d

if __name__ == "__main__":
    split = 5
    train = []
    test = []
    for gest in gestures:
        file = open('/scratch/kinect_data/'+gest+'.depth','r')
        depthFrames = cPickle.load(file)
        for fn in range(0,split):
            frame = depthFrames[fn]
            frame = frame.astype('uint8')
            test.append(getFeatures(frame, gest))
        for fn in range(split,len(depthFrames)):
            frame = depthFrames[fn]
            frame = frame.astype('uint8')
            train.append(getFeatures(frame, gest))
        file.close()

    conf = [[0]*len(gestures)+[i] for i in gestures]
    for te in test:
        md = [-1]*k
        mv = ['']*k
        for tr in train:
            d = dist(te,tr,act)
            for i in range(k):
                if md[i] == -1 or d < md[i]:
                    md[i] = d
                    mv[i] = tr[3]
        mt = 0
        mc = ''
        for c in gestures:
            t = mv.count(c)
            if t > mt:
                mt = t
                mc = c
        
        conf[gestures.index(te[3])][gestures.index(mc)] += 1

    for r in conf:
        print r

